#include "JY62.h"
#include "sensor.h"

static uint8_t RxBuffer[15];/*接收数据数组*/
uint8_t jy62_data;
static volatile uint8_t RxState = 0;/*接收状态标志位*/
static uint8_t RxIndex = 0;/*接受数组索引*/
float Roll,Pitch,Yaw;/*角度信息，如果只需要整数可以改为整数类型*/
float  aX,aY,aZ;//三维加速度
float wX,wY,wZ;//三维角速度
float Yaw_target = 0; //目标航向角

float l_pwm,r_pwm;

void jy61p_ReceiveData(uint8_t RxData)
{
uint8_t i,sum=0;
	//状态机机制，使用RxState跟踪接收状态
	if (RxState == 0)	//等待包头
	{
		if (RxData == 0x55)	//收到包头
		{
			RxBuffer[RxIndex] = RxData;
			RxState = 1;
			RxIndex = 1;	//进入下一状态
		}
	}
	else if (RxState == 1)//确认数据类型
	{
		if (RxData == 0x53)	/*判断数据内容，修改这里可以改变要读的数据内容，0x53为角度输出*/
		{
			RxBuffer[RxIndex] = RxData;
			RxState = 2;
			RxIndex = 2;	//进入下一状态
		}
		if (RxData == 0x51)	/*判断数据内容，修改这里可以改变要读的数据内容，0x51为加速度输出*/
		{
			RxBuffer[RxIndex] = RxData;
			RxState = 3;
			RxIndex = 2;	//进入下一状态
		}
		if (RxData == 0x52)	/*判断数据内容，修改这里可以改变要读的数据内容，0x52为角速度输出*/
		{
			RxBuffer[RxIndex] = RxData;
			RxState = 4;
			RxIndex = 2;	//进入下一状态
		}
	}
	else if (RxState == 2)	//接收数据并解析
	{
		RxBuffer[RxIndex++] = RxData;
		if(RxIndex == 11)	//接收完成
		{
			for(i=0;i<10;i++)
			{
				sum = sum + RxBuffer[i];	//计算校验和
			}
			if(sum == RxBuffer[10])		//校验成功
			{
				/*计算数据，根据数据内容选择对应的计算公式*/
				Roll = ((int16_t) ((int16_t) RxBuffer[3] << 8 | (int16_t) RxBuffer[2])) / 32768.0f * 180.0f;
				Pitch = ((int16_t) ((int16_t) RxBuffer[5] << 8 | (int16_t) RxBuffer[4])) / 32768.0f * 180.0f;
				Yaw = ((int16_t) ((int16_t) RxBuffer[7] << 8 | (int16_t) RxBuffer[6])) / 32768.0f * 180.0f;
			}
			RxState = 0;
			RxIndex = 0;	//读取完成，回到最初状态，等待包头
		}
	}
	else if (RxState == 3)	//接收数据
	{
		RxBuffer[RxIndex++] = RxData;
		if(RxIndex == 11)	//接收完成
		{
			for(i=0;i<10;i++)
			{
				sum = sum + RxBuffer[i];	//计算校验和
			}
			if(sum == RxBuffer[10])		//校验成功
			{
				/*计算数据，根据数据内容选择对应的计算公式*/
				aX = ((int16_t) ((int16_t) RxBuffer[3] << 8 | (int16_t) RxBuffer[2])) / 32768.0f * 16.0f;
				aY = ((int16_t) ((int16_t) RxBuffer[5] << 8 | (int16_t) RxBuffer[4])) / 32768.0f * 16.0f;
				aZ = ((int16_t) ((int16_t) RxBuffer[7] << 8 | (int16_t) RxBuffer[6])) / 32768.0f * 16.0f;
			}
			RxState = 0;
			RxIndex = 0;	//读取完成，回到最初状态，等待包头
		}
	}
	else if (RxState == 4)	//接收数据
	{
		RxBuffer[RxIndex++] = RxData;
		if(RxIndex == 11)	//接收完成
		{
			for(i=0;i<10;i++)
			{
				sum = sum + RxBuffer[i];	//计算校验和
			}
			if(sum == RxBuffer[10])		//校验成功
			{
				/*计算数据，根据数据内容选择对应的计算公式*/
				wX = ((int16_t) ((int16_t) RxBuffer[3] << 8 | (int16_t) RxBuffer[2])) / 32768.0f * 2000.0f;
				wY= ((int16_t) ((int16_t) RxBuffer[5] << 8 | (int16_t) RxBuffer[4])) / 32768.0f * 2000.0f;
				wZ = ((int16_t) ((int16_t) RxBuffer[7] << 8 | (int16_t) RxBuffer[6])) / 32768.0f * 2000.0f;
			}
			RxState = 0;
			RxIndex = 0;	//读取完成，回到最初状态，等待包头
		}
	}

}

void jy62_init(void)
{
	JY62_PID.Kp = 0.00f;//5.1
	JY62_PID.Ki = 0.00f; 
	JY62_PID.Kd = 0.00f; //0.2
	JY62_PID.integral_max = 30;
	
}



void jy62_proc(void)
{	
    float calc_val = PID_Calc(&JY62_PID, Yaw_target, Yaw,1);//如果算的YawPID都要加1，其余为0
	float L = -100 - calc_val;
	float R = -100 + calc_val;
    motor_speed_set(L,R);
	//printf("%.3f,%.3f,%.2f\n",L,R,Yaw_target);
}








